/*
 * @Description: 梯形路径插补算法，速度插补分为四部分
 *                  匀加速阶段ta
 *                  匀速阶段tc
 *                  匀减速阶段td
 *                  低速阶段ts
 * @Author: Dryad
 * @Date: 2019-06-27 10:00:28
 */
#pragma once

class Interpolation_Class
{
public:
    Interpolation_Class() : interpolation_state(NO_Interpolation) {}

    /* 路径插补状态枚举 */
    enum Interpolation_State_Enum
    {
        NO_Interpolation,  /* 还未进行插补 */
        IS_Interpolating,  /* 正在插补 */
        IS_Interpolated,   /* 插补完成 */
    } interpolation_state; /* 插补状态 */

    /* 路径插补参数 */
    struct Interpolation_Parameter_Struct
    {
        float threshold;    /* 插补阈值，移动距离低于该阈值，认为已经插补完成 */
        float min_velocity; /* 插补时最小速度(mm/s) */
        float max_velocity; /* 插补时最大速度(mm/s)*/
        float acc;          /* 加速度(mm/s2) */
        float dec;          /* 减速度(mm/s2) */
        float slow_time;    /* 低速时间(s) */
        void *user_data;    /* 保留选型，此处用于连接Bezier曲线第一个点 */
    };

    /* 根据路径总长度和插补参数规划速度曲线，若移动距离小于阈值，返回false */
    bool Init(float sum_distance);
    float Cal_Velocity(const float current_distance); /* 根据当前位移计算插补速度 */

    struct Interpolation_Parameter_Struct interpolation_parameter; /* 插补参数 */
private:
    static float origin_velocity; /* 起点速度(mm/s)(°/s) */

    static float acc_distance;   /* 加速度段距离(mm)(°) */
    static float const_distance; /* 匀速度段距离(mm)(°) */
    static float dec_distance;   /* 减速度段距离(mm)(°) */
    static float slow_distance;  /* 低速段距离(mm)(°) */

    static float acc_time;   /* 加速段时间(s) */
    static float const_time; /* 匀速段时间(s) */
    static float dec_time;   /* 减速段时间(s) */
    static float slow_time;  /* 低速段时间(s) */
};
